#include "velpose_sim_core.h"

VelPoseSim::VelPoseSim():nh_(""), private_nh_("~"), listener(buffer){
    private_nh_.param<double>("init_x", init_x, 0.0);
    private_nh_.param<double>("init_y", init_y, 0.0);
    private_nh_.param<double>("init_yaw", init_yaw, 0.0);
    private_nh_.param<std::string>("input_ctrl_cmd_topic_", input_ctrl_cmd_topic_, "");
}

VelPoseSim::~VelPoseSim(){}

void VelPoseSim::run(){
    ctrl_cmd_sub_ = nh_.subscribe<geometry_msgs::Twist>(input_ctrl_cmd_topic_, 1, &VelPoseSim::ctrl_cmd_callback, this);
    /* 订阅initial_pose*/
    initial_pose_sub = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, &VelPoseSim::initial_pose_callback, this);
    ros::Timer timer = nh_.createTimer(ros::Duration(0.1), &VelPoseSim::process, this, false, true);
    ros::spin();
}

void VelPoseSim::ctrl_cmd_callback(const geometry_msgs::Twist::ConstPtr& msg){
    geometry_msgs::TwistStamped twist_;
    twist_.header.stamp = ros::Time().now();
    twist_.header.frame_id = "base_link";
    twist_.twist = *msg;
    ctrl_cmd_ = std::make_shared<geometry_msgs::TwistStamped>(twist_);
}

void VelPoseSim::initial_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg){
    init_x = msg->pose.pose.position.x;
    init_y = msg->pose.pose.position.y;
    init_yaw = tf::getYaw(msg->pose.pose.orientation);
}

void VelPoseSim::process(const ros::TimerEvent& e){
    double vx, angular;
    if(ctrl_cmd_==nullptr || fabs(ros::Time().now().toSec()-ctrl_cmd_->header.stamp.toSec())>1.0){
        vx = 0.0;
        angular = 0.0;
    }else{
        vx = ctrl_cmd_->twist.linear.x;
        angular = ctrl_cmd_->twist.angular.z;
    }

    double x,y,z,yaw;

    x = init_x + vx*0.1*cos(init_yaw);
    y = init_y + vx*0.1*sin(init_yaw);
    z = 0.2;
    yaw = init_yaw + angular*0.1;

    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << x, y, z;
    transform.rotate(Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()));
    Eigen::Matrix4f mat_pose = transform.matrix();

    geometry_msgs::TransformStamped map_wrt_frame = tf2::eigenToTransform(Eigen::Isometry3d(mat_pose.inverse().cast<double>()));
    map_wrt_frame.header.stamp = ros::Time().now();
    map_wrt_frame.header.frame_id = "base_link";
    map_wrt_frame.child_frame_id = "map";

    geometry_msgs::TransformStamped frame_wrt_odom = buffer.lookupTransform("base_link", "base_link", ros::Time(0));
    Eigen::Matrix4f frame2odom = tf2::transformToEigen(frame_wrt_odom).cast<float>().matrix();

    geometry_msgs::TransformStamped map_wrt_odom;
    tf2::doTransform(map_wrt_frame, map_wrt_odom, frame_wrt_odom);

    tf2::Transform odom_wrt_map;
    tf2::fromMsg(map_wrt_odom.transform, odom_wrt_map);
    odom_wrt_map = odom_wrt_map.inverse();

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.transform = tf2::toMsg(odom_wrt_map);
    odom_trans.header.stamp = ros::Time().now();
    odom_trans.header.frame_id = "map";
    odom_trans.child_frame_id = "base_link";
    transformBroadcaster.sendTransform(odom_trans);

    init_x = x;
    init_y = y;
    init_yaw = yaw;    
}

